/*
 * Track the dot of a red laser pointer on images
 */

#include <ros/ros.h>
#include "sensor_msgs/Image.h"
#include "image_transport/image_transport.h"
#include "geometry_msgs/Polygon.h"
#include "cv_bridge/CvBridge.h"
#include "opencv2/core/core.hpp"
#include "opencv2/highgui/highgui.hpp"

#include "rosqwerk/DigitalOutputsControl.h"

#include <stdlib.h>
#include <stdio.h>
#include <string>
#include <stack>
#include <list>

using namespace std;
void onMouseEvent(int event, int x, int y, int flags, void *param);

class LaserTracker {

protected:
	deque<cv::Mat> _images;
	bool _freeze;

public:

	LaserTracker() :
		_freeze(false) {

		cvSetMouseCallback("Image", onMouseEvent, this);
	}

	~LaserTracker() {
	}

	void processImage(cv::Mat image, bool laserIsOn) {
		CV_Assert(image.type() == CV_8UC3);

		cv::Mat redImg;
		cv::Mat brightImg;
		computeRednessImage(image, redImg, brightImg);

		cv::dilate(brightImg, brightImg, cv::Mat(), cv::Point(-1, -1), 1);
		cv::imshow("bright", brightImg);

		cv::dilate(redImg, redImg, cv::Mat(), cv::Point(-1, -1), 3);
		cv::imshow("red", redImg);

		cv::Mat brightSurroundedRedImg;
		cv::bitwise_and(brightImg, redImg, brightSurroundedRedImg);
		cv::erode(brightSurroundedRedImg, brightSurroundedRedImg, cv::Mat(),
				cv::Point(-1, -1), 1);
		cv::imshow("brightSurroundedRedImg", brightSurroundedRedImg);

		cv::Mat matchLaserDotImg;
		computeMatchingWithLaserDotUsingFilter(brightSurroundedRedImg, matchLaserDotImg);
		cv::imshow("matchLaserDotImg", matchLaserDotImg);

		// select best candidates
		map<float, cv::Point> bestCandidates;
		double maxVal = 0;
		for (int y = 0; y < matchLaserDotImg.rows; y++) {
			float* row = matchLaserDotImg.ptr<float> (y);
			for (int x = 0; x < matchLaserDotImg.cols; x++) {
				float v = row[x];
				if (v > maxVal) {
					maxVal = v;
					bestCandidates[v] = cv::Point(x, y);
					if (bestCandidates.size() > 3) {
						// erase last: as map are ordered by value, begin() is the min value in the
						// map
						//cout << "erase: size before:" << bestCandidates.size() << endl;
						bestCandidates.erase(bestCandidates.begin());
						//cout << "erase: size after:" << bestCandidates.size() << endl;
						//bestCandidates.erase(bestCandidates.begin());
					}
				}
			}
		}

		//cout << "candidates:";
		int color = 255;
		for (map<float, cv::Point>::iterator it = bestCandidates.begin(); it
				!= bestCandidates.end(); it++) {
			cv::Point pt = (*it).second;
			//cout << (*it).first << ";";
			cv::circle(image, pt, 4, cv::Scalar(0, color, 0));
			color -= 10;
		}
		//cout << endl;

		cv::imshow("currentImage", image);

		//		// add to our images list:
		//		_images.push_front(redImg);
		//
		//		// now diffs the images to get the laser dot.
		//		cv::Mat accWithLaser;
		//		cv::Mat accWithoutLaser;
		//		cv::Mat diffImg;
		//		bool ok = computeDiffsBetweenImages(laserIsOn, accWithLaser,
		//				accWithoutLaser, diffImg);
		//		if (ok) {
		//			cv::imshow("accWithLaser", accWithLaser);
		//			cv::imshow("accWithoutLaser", accWithoutLaser);
		//			cv::imshow("diffImage", diffImg);
		//		}
		//
		//		// remove older image:
		//		if (_images.size() >= 2) {
		//			_images.pop_back();
		//		}

		// let opencv display images
		cv::waitKey(3);
	}

	bool computeDiffsBetweenImages(bool laserIsOn, cv::Mat& accWithLaser,
			cv::Mat& accWithoutLaser, cv::Mat& diff) {
		// diffs the images even/odd to get the laser dot.
		// Do this on several images, to differentiate from regular movements
		// in images (that are supposed more of less random)
		cv::Mat img = _images.front();
		cv::Mat evenImg(img.rows, img.cols, CV_32FC1);
		cv::Mat oddImg(img.rows, img.cols, CV_32FC1);
		for (deque<cv::Mat>::iterator it = _images.begin(); it != _images.end();) {
			cv::Mat i1 = *(it++);
			if (it != _images.end()) {
				cv::Mat i2 = *(it++);
				if ((!i1.empty()) && (!i2.empty())) {
					cv::accumulate(i1, evenImg);
					cv::accumulate(i2, oddImg);
				}
			}
		}
		if (!evenImg.empty()) {
			cv::Mat diffImg(img.rows, img.cols, CV_32FC1);
			cv::absdiff(evenImg, oddImg, diffImg);

			float scale = (1.0f / (255.0 * _images.size() / 2));
			evenImg.convertTo(evenImg, CV_32FC1, scale);
			oddImg.convertTo(oddImg, CV_32FC1, scale);
			if (!laserIsOn) {
				accWithLaser = evenImg;
				accWithoutLaser = oddImg;
			} else {
				accWithLaser = oddImg;
				accWithoutLaser = evenImg;
			}

			diffImg.convertTo(diff, CV_32FC1, (1.0f / 255.0));
			return true;
		}
		return false;
	}

	void getObjectColor(int event, int x, int y) {
		//		if (event == CV_EVENT_LBUTTONUP) {
		//			if (!_currentImage.empty()) {
		//				cv::Vec3b pixel = _currentImage.at<cv::Vec3b> (y, x);
		//				printf("pixel h=%d,s=%d,v=%d\n", (int) pixel.val[0],
		//						(int) pixel.val[1], (int) pixel.val[2]);
		//			} else {
		//				printf("null param\n");
		//			}
		//		} else if (event == CV_EVENT_RBUTTONUP) {
		//			_freeze = !_freeze;
		//		}
	}

	/*
	 void processImage(const cv::Mat& image) {


	 // --- match template ---
	 uchar laserDotV[]={
	 10 , 10, 10, 10, 10, 10, 10, 10,
	 10 , 10, 80,100, 80, 10, 10, 10,
	 10 , 80,100,100,100, 80, 10, 10,
	 10 ,100,255,255,255,100, 10, 10,
	 10 ,100,255,255,255,100, 10, 10,
	 10 , 80,100,100,100, 80, 10, 10,
	 10 , 10, 80,100, 80, 10, 10, 10,
	 10 , 10, 10, 10, 10, 10, 10, 10};
	 cv::Mat laserDotImg(8, 8, CV_8UC1,laserDotV);

	 cv::Mat matchingImg(img.rows, img.cols, CV_32FC1);

	 cv::matchTemplate(redImg,laserDotImg,matchingImg,CV_TM_CCORR_NORMED);

	 //float v1=matchingImg.at<float>(10,10);
	 //printf("toto %f\n",v1);
	 cv::threshold(matchingImg,matchingImg,0.80,255.,CV_THRESH_TOZERO);

	 cv::imshow("ImageMatch", matchingImg);
	 cv::waitKey(3);

	 // ---- circles ----
	 std::vector<cv::Vec3f> circles;
	 cv::HoughCircles(redImg, circles, CV_HOUGH_GRADIENT,
	 2, redImg.rows/4, 200, 100, 2, 4);

	 cv::Mat circlesImg(img.rows, img.cols, CV_8UC1);
	 for( size_t i = 0; i < circles.size(); i++ )
	 {
	 cv::Point center(cvRound(circles[i][0]), cvRound(circles[i][1]));
	 int radius = cvRound(circles[i][2]);
	 // draw the circle center
	 cv::circle( circlesImg, center, 1, cv::Scalar(0,255,0));
	 // draw the circle outline
	 cv::circle( circlesImg, center, radius, cv::Scalar(0,0,255));
	 }

	 cv::imshow("ImageCircle", circlesImg);
	 cv::waitKey(3);

	 // ---- simple filter ----
	 float laserDotFV[]={
	 -5 , -5, -5, -5, -5, -5, -5, -5, -5,
	 -5 , -5, -5, -5, -5, -5, -5, -5, -5,
	 -5 , -5,  1,  2,  2,  2,  1, -5, -5,
	 -5 ,  1,  2,  4,  4,  4,  2,  1, -5,
	 -5 ,  2,  4,  8,  8,  8,  4,  2, -5,
	 -5 ,  2,  4,  8,  8,  8,  4,  2, -5,
	 -5 ,  1,  2,  4,  4,  4,  2,  1, -5,
	 -5 , -5,  1,  2,  2,  2,  1, -5, -5,
	 -5 , -5, -5, -5, -5, -5, -5, -5, -5};
	 cv::Mat laserDotFImg(9, 9, CV_32FC1, laserDotFV);

	 cv::Mat filteredImg(img.rows, img.cols, CV_8UC1);

	 cv::filter2D(redImg, filteredImg, -1, laserDotFImg);
	 cv::threshold(filteredImg,filteredImg,254.,255.,cv::THRESH_BINARY);
	 cv::imshow("ImageFilter", filteredImg);
	 cv::waitKey(3);
	 }
	 */

	void computeRednessImage(const cv::Mat& src, cv::Mat& redImgP,
			cv::Mat& brightImgP) {
		CV_Assert(src.type() == CV_8UC3);

		// go to HSV to limit lightning conditions effects:
		cv::Mat img = src.clone();
		cv::cvtColor(img, img, CV_BGR2HSV);

		// for each pixel, tell if it is red or bright:
		cv::MatIterator_<cv::Vec3b> it = img.begin<cv::Vec3b> ();
		cv::MatIterator_<cv::Vec3b> it_end = img.end<cv::Vec3b> ();
		for (; it != it_end; ++it) {
			cv::Vec3b& pix = *it;
			int h = (int) pix[0];
			int s = (int) pix[1];
			int v = (int) pix[2];
			int distFromRed = (h > 90) ? (180 - h) : (h);
			bool isRed = false;
			bool isBright = false;
			if (v >= 254) {
				// very bright
				isBright = true;
			} else if ((v > 90) && (s > 125) && (distFromRed <= 10)) {
				// quite red?
				isRed = true;
			}
			pix[0] = (uchar) (isBright ? 255 : 0);
			pix[1] = (uchar) (isRed ? 255 : 0);
			pix[2] = (uchar) 0;
		}

		cv::Mat redImg(img.rows, img.cols, CV_8UC1);
		cv::Mat brightImg(img.rows, img.cols, CV_8UC1);
		cv::Mat out[] = { brightImg, redImg };
		int from_to[] = { 0, 0, 1, 1 };
		cv::mixChannels(&img, 1, out, 2, from_to, 2);

		redImgP = redImg;
		brightImgP = brightImg;
	}

	void computeMatchingWithLaserDot(const cv::Mat& src, cv::Mat& dst) {
		// --- match template ---
		uchar v01[] = { 000, 000, 000, 000, 000, 000, 000, 000, 000, 000, 000 };
		uchar v02[] = { 000, 000, 000, 000, 000, 000, 000, 000, 000, 000, 000 };
		uchar v03[] = { 000, 000, 000, 000, 000, 255, 000, 000, 000, 000, 000 };
		uchar v04[] = { 000, 000, 000, 000, 255, 255, 255, 000, 000, 000, 000 };
		uchar v05[] = { 000, 000, 000, 255, 255, 255, 255, 255, 000, 000, 000 };
		uchar v06[] = { 000, 000, 000, 255, 255, 255, 255, 255, 000, 000, 000 };
		uchar v07[] = { 000, 000, 000, 255, 255, 255, 255, 255, 000, 000, 000 };
		uchar v08[] = { 000, 000, 000, 000, 255, 255, 255, 000, 000, 000, 000 };
		uchar v09[] = { 000, 000, 000, 000, 000, 255, 000, 000, 000, 000, 000 };
		uchar v10[] = { 000, 000, 000, 000, 000, 000, 000, 000, 000, 000, 000 };
		uchar v11[] = { 000, 000, 000, 000, 000, 000, 000, 000, 000, 000, 000 };
		cv::Mat laserDotImg(11, 11, CV_8UC1);
		laserDotImg.row(0) = cv::Mat(11, 1, CV_8UC1, v01);
		laserDotImg.row(1) = cv::Mat(11, 1, CV_8UC1, v02);
		laserDotImg.row(2) = cv::Mat(11, 1, CV_8UC1, v03);
		laserDotImg.row(3) = cv::Mat(11, 1, CV_8UC1, v04);
		laserDotImg.row(4) = cv::Mat(11, 1, CV_8UC1, v05);
		laserDotImg.row(5) = cv::Mat(11, 1, CV_8UC1, v06);
		laserDotImg.row(6) = cv::Mat(11, 1, CV_8UC1, v07);
		laserDotImg.row(7) = cv::Mat(11, 1, CV_8UC1, v08);
		laserDotImg.row(8) = cv::Mat(11, 1, CV_8UC1, v09);
		laserDotImg.row(9) = cv::Mat(11, 1, CV_8UC1, v10);
		laserDotImg.row(10) = cv::Mat(11, 1, CV_8UC1, v11);


		cv::Mat matchingImg(src.rows, src.cols, CV_32FC1);
		cv::matchTemplate(src, laserDotImg, matchingImg, cv::TM_CCORR_NORMED);

		cv::threshold(matchingImg, matchingImg, 0.5, 255., cv::THRESH_TOZERO);

		dst = matchingImg;
	}

	void computeMatchingWithLaserDotUsingFilter(const cv::Mat& src, cv::Mat& dst) {
		// --- match template ---
		float v01[] = { -4, -4, -4, -4, -4, -4, -4, -4, -4, -4, -4,
		 -4, -4, -4, -4, 04, 04, -4, -4, -4, -4, -4,
		 -4, -4, -4, 04, 04, 04, 04, -4, -4, -4, -4,
		 -4, -4, -4, 04, 04, 04, 04, -4, -4, -4, -4,
		 -4, -4, 04, 04,  8,  8, 04, 04, -4, -4, -4,
		 -4, -4, 04, 04,  8,  8, 04, 04, -4, -4, -4,
		 -4, -4, -4, 04, 04, 04, 04, -4, -4, -4, -4,
		 -4, -4, -4, 04, 04, 04, 04, -4, -4, -4, -4,
		 -4, -4, -4, -4, 04, 04, -4, -4, -4, -4, -4,
		 -4, -4, -4, -4, -4, -4, -4, -4, -4, -4, -4,
		 -4, -4, -4, -4, -4, -4, -4, -4, -4, -4, -4 };
		cv::Mat laserDotImg(11, 11, CV_32FC1, v01);
		//double sum=cv::sum(laserDotImg)[0];

	 cv::Mat matchingImg(src.rows, src.cols, CV_32FC1);
	 cv::filter2D(src, matchingImg, CV_32FC1, laserDotImg);
	 cv::normalize(matchingImg,dst,10);
	 //dst=matchingImg;
	}

	//	void processImage(const cv::Mat& image) {
	//		CV_Assert(image.type() == CV_8UC3);
	//		cv::imshow("Image", image);
	//
	//		if (!_freeze) {
	//			image.copyTo(_image);
	//		}
	//
	//		cv::Mat redImg;
	//      computeRednessImage(_image,redImg);
	//		cv::imshow("ImageR", redImg);
	//
	//		cv::Mat matchingImg;
	//      computeMatchingWithLaserDot(redImg,matchingImg);
	//		cv::imshow("ImageMatch", matchingImg);
	//
	//		cv::waitKey(3);
	//	}
};

void onMouseEvent(int event, int x, int y, int flags, void *param) {
	LaserTracker* tracker = (LaserTracker*) param;
	if (tracker != NULL) {
		tracker->getObjectColor(event, x, y);
	}
}

class LaserTrackerNode {

protected:

	ros::NodeHandle _node;
	sensor_msgs::CvBridge _cvBridge;

	image_transport::ImageTransport _imageTransport;

	image_transport::Subscriber _imageSubscriber;
	ros::Publisher _boundsPublisher;
	ros::Publisher _digitalOutputsControlPublisher;

	LaserTracker& _laserTracker;
	bool _laserIsOn;
	int _laserToggleFrameCount;

public:

	LaserTrackerNode(LaserTracker& laserTracker) :
		_imageTransport(_node), _laserTracker(laserTracker), _laserIsOn(false),
				_laserToggleFrameCount(0) {
		ros::NodeHandle local_params("~");
		std::string imageTopic;
		local_params.param<std::string> ("image_topic", imageTopic, "/image");

		_boundsPublisher = _node.advertise<geometry_msgs::Polygon> (
				"/lasertracker/boundbox", 1);
		_imageSubscriber = _imageTransport.subscribe(imageTopic, 1,
				&LaserTrackerNode::imageCallback, this);
		printf("laser tracker started on images from '%s'\n",
				imageTopic.c_str());

		_digitalOutputsControlPublisher = _node.advertise<
				rosqwerk::DigitalOutputsControl> (
				"/qwerk/digitaloutputs_control", 1);
	}

	~LaserTrackerNode() {
		setLaserState(false);
	}

	void toggleLaser() {
		//if ((--_laserToggleFrameCount) <= 0) {
		//	_laserToggleFrameCount = 1;
		_laserIsOn = !_laserIsOn;
		setLaserState(_laserIsOn);
		//}
	}

	void setLaserState(bool laserIsOn) {
		rosqwerk::DigitalOutputsControl msg;
		boost::array<bool, 8> outMask = { { true, true, false, false, false,
				false, false, false } };
		msg.digitalOutMask = outMask;
		boost::array<bool, 8> outStates = { { laserIsOn, laserIsOn, false,
				false, false, false, false, false } };
		msg.digitalOutStates = outStates;
		_digitalOutputsControlPublisher.publish(msg);
	}

	void imageCallback(const sensor_msgs::ImageConstPtr& msg_ptr) {
		// toggle laser first, to be ready for the next frame:
		toggleLaser();

		// now process image:
		IplImage* cv_image = NULL;
		try {
			cv_image = _cvBridge.imgMsgToCv(msg_ptr);
			_laserTracker.processImage(cv::Mat(cv_image), _laserIsOn);
		} catch (sensor_msgs::CvBridgeException error) {
			ROS_ERROR("error");
		}

	}
};

int main(int argc, char** argv) {
	bool useROS = true;

	LaserTracker tracker;
	if (useROS) {
		ros::init(argc, argv, "lasertracker");
		LaserTrackerNode trackerNode(tracker);
		ros::spin();
	} else {
		//cv::VideoCapture cap(0); // webcam 0
		cv::VideoCapture cap("data/fred_with_laser_dot.ogv");
		if (!cap.isOpened()) {
			std::cerr << "no video input" << std::endl;
			return -1;
		}
		cv::Mat frame;
		while (true) {
			cap >> frame;
			tracker.processImage(frame, false);
		}

	}
	return 0;
}

/*
 void processImage(const cv::Mat& image) {
 CV_Assert(image.type() == CV_8UC3);

 cv::imshow("Image", image);
 cv::waitKey(3);


 // coeur laser (sur feuille verte):
 // pixel h=139,s=15,v=254
 // pourtour:
 // pixel h=175,s=160,v=110
 // pixel h=153,s=173,v=150

 // sur bois:
 // coeur:
 // pixel h=53,s=11,v=254
 // pourtour:
 // pixel h=2,s=134,v=250

 //printf("laser tracker image received\n");
 if (!_freeze) {
 image.copyTo(_image);
 cv::cvtColor(_image, _image, CV_BGR2HSV);
 }

 //cvSmooth(_image,_image,CV_BLUR);
 cv::imshow("ImageHSV", _image);
 cv::waitKey(3);

 //double hmin = 160.0;
 // double hmax = 180.0;
 // double smin = 100.0;
 // IplImage *mask = cvCreateImage(cvGetSize(_image), _image->depth, 1);
 // cvInRangeS(_image, cvScalar(hmin, smin, 0),
 // cvScalar(hmax, 255, 255), mask);
 // cvShowImage("ImageR", mask);
 // cvWaitKey(10);
 // cvReleaseImage(&mask);

 cv::Mat img = _image.clone();
 cv::MatIterator_<cv::Vec3b> it = img.begin<cv::Vec3b> ();
 cv::MatIterator_<cv::Vec3b> it_end = img.end<cv::Vec3b> ();
 for (; it != it_end; ++it) {
 cv::Vec3b& pix = *it;

 //for (int y = 0; y < img->height; y++) {
 //	uchar* ptr = (uchar*) (img->imageData + y * img->widthStep);
 //	for (int x = 0; x < img->width; x++) {
 //		int off = 3 * x;
 //		int h = (int) ptr[off + 0];
 //		int s = (int) ptr[off + 1];
 //		int v = (int) ptr[off + 2];
 int h = (int) pix[0];
 int s = (int) pix[1];
 int v = (int) pix[2];

 pix[0] = (uchar) 0;
 pix[1] = (uchar) 0;
 pix[2] = (uchar) 0;

 // red halo:
 if ((s >= 110) && ((h >= 170) || (h <= 10)) && (v >= 100)) {
 pix[0] = (uchar) 255;
 pix[1] = (uchar) 0;
 pix[2] = (uchar) 255;
 }

 // white core:
 if ((v >= 252) && ((h >= 130) || (h <= 50))) {
 pix[0] = (uchar) 0;
 pix[1] = (uchar) 255;
 pix[2] = (uchar) 255;
 }
 }

 cv::imshow("ImageR", img);
 cv::waitKey(3);

 cv::Mat redImg(img.rows, img.cols, CV_8UC1);
 cv::Mat highImg(img.rows, img.cols, CV_8UC1);
 cv::Mat out[] = { redImg, highImg };
 int from_to[] = { 0, 0, 1, 1 };
 cv::mixChannels(&img, 1, out, 2, from_to, 2);

 //cvSmooth(trackingImg,trackingImg);
 //			cvShowImage("ImageHSVSmooth", trackingImg);
 //					cvWaitKey(10);
 //cvErode(trackingImg,trackingImg,NULL,2);
 cv::erode(redImg, redImg, cv::Mat(), cv::Point(-1, -1), 1);
 cv::dilate(redImg, redImg, cv::Mat(), cv::Point(-1, -1), 4);
 cv::imshow("ImageRed", redImg);
 cv::waitKey(3);

 cv::erode(highImg, highImg, cv::Mat(), cv::Point(-1, -1), 2);
 cv::dilate(highImg, highImg, cv::Mat(), cv::Point(-1, -1), 3);
 cv::imshow("ImageHigh", highImg);
 cv::waitKey(3);

 cv::bitwise_and(highImg, redImg, highImg);
 cv::imshow("ImageRedAndHigh", highImg);
 cv::waitKey(3);

 //try {
 //	geometry_msgs::Polygon boxes;
 //	boxes.set_points_size(2);
 //	boxes.points[0].x = rect.x;
 //	boxes.points[0].y = rect.y;
 //	boxes.points[1].x = rect.x + rect.width;
 //	boxes.points[1].y = rect.y + rect.height;
 //	_boundsPub.publish(boxes);
 //} catch (sensor_msgs::CvBridgeException error) {
 //	ROS_ERROR("error");
 //}
 }
 */

